//OPENCV
#include <highgui.h>
#include <cv.h>
#include <cvaux.h>

//STD
#include <iostream>
#include <vector>

using namespace std;

//Proprietary
#include "clsSegment.hpp"
#include "plotAPI.h"
#include "windowsTracking.hpp"
#include "clsVirtualJoystick.hpp"

// Mouse event parameters
CvPoint segPos;
bool mouseEvent = false;

// Calibration struct
typedef struct{
  double minH, maxH, minS, maxS, minV, maxV;
  int x, y;
} calibration;


void on_mouse( int event, int x, int y, int flags , void(*))
{
    flags = flags;
    
    switch( event )
      {
      case CV_EVENT_LBUTTONDOWN:
	segPos.x = x;
	segPos.y = y;
	mouseEvent = true;
	break;	
    }
}


int main (void)
{
  CvCapture *OpenCV_CAM;
  
  OpenCV_CAM = cvCaptureFromCAM(-1);

  IplImage* src;
  IplImage* srcHighRes;  
  IplImage* dst;
  IplImage* tmp;
  IplImage* hsv;
  IplImage* hue;
  IplImage* sat;
  IplImage* val;
  IplImage* movement;
  IplImage* tracked;
  IplImage* trackedS;
  IplImage* trackedH;
  IplImage* trackedV;

  if (!OpenCV_CAM)
    {
      cout << " Error with capturing device!!" << endl;
      exit (-1);
    }
  
  cvSetCaptureProperty( OpenCV_CAM, CV_CAP_PROP_FRAME_HEIGHT, 240);
  cvSetCaptureProperty( OpenCV_CAM, CV_CAP_PROP_FRAME_WIDTH, 320 );

  srcHighRes = cvQueryFrame( OpenCV_CAM );
  src = cvCreateImage(cvSize(320,240), IPL_DEPTH_8U, 3);


  dst = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 3);
  tmp = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  movement = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  hsv = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 3);
  hue = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  sat = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  val = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  tracked = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  trackedS = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  trackedH = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
  trackedV = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);

  int minTh = 90;
  int width = 80;
  int THRESHOLD = 230;
  int smooth = 3;

  cvNamedWindow("EasyTrack input", CV_WINDOW_AUTOSIZE );
  cvCreateTrackbar("Sensitivity","EasyTrack input",&THRESHOLD,254,NULL);
  cvCreateTrackbar("Noise Filter","EasyTrack input",&smooth,40,NULL);

  CvHistogram *H = 0;
  CvHistogram *S = 0;
  CvHistogram *V = 0;

  float hranges_arr[] = {0,180};
  float* hranges = hranges_arr;

  float hranges_arrSV[] = {0,255};
  float* hrangesSV = hranges_arrSV;
  int hdims = 16;
    
  H = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
  S = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hrangesSV, 1 );
  V = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hrangesSV, 1 );

  // Mouse click updating tracking position
  cvSetMouseCallback("EasyTrack input", &on_mouse);

  clsSegment* segment = new (clsSegment);
  
  windowTracking element = windowTracking ((char*)"NO_NAME",src->width, 100, src->width/2, src->height/2);

  segPos.x = src->width/2;
  segPos.y = src->height/2;
  
  segment->init(src);

  // Tracking init
  CvRect track_window;
  CvBox2D track_box;

  // Joystick init
  virtualJoystick* js0 = new (virtualJoystick);

  track_window.x = src->width/2;
  track_window.y = src->height/2;
  track_window.width = src->height/6;
  track_window.height = src->width/6;

  double widthE = 0, heightE = 0;

  double minH, maxH, minS, maxS, minV, maxV;

  vector <calibration> list;

//   // Create video
//   CvVideoWriter*          pBGAvi = NULL;
//   pBGAvi=cvCreateVideoWriter("test.avi",CV_FOURCC('x','v','i','d'),13,cvSize(src->width,src->height) );

  for(;;)
    {
      //cvZero (tmp);
      cvZero (trackedH);
      cvZero (trackedS);
      cvZero (trackedV);
      cvZero (tracked);

      // Resize image to QVGA
      srcHighRes = cvQueryFrame( OpenCV_CAM );
      if ((srcHighRes->width != 320) || (srcHighRes->height != 240))
	{
	  cvResize( srcHighRes, src);
	}
      else
	//srcHighRes = src;
	src = srcHighRes;

      if (mouseEvent)
	{
	  segment->reset(src);
	  segment->setParams (minTh, 0);
	  segment->process (src, dst, segPos, width);

	  segment->getHists(H,S,V);

	  // Reset tracked element
	  element.init_traces (1000);
	  element.position.x_pos = segPos.x;
	  element.position.y_pos = segPos.y;
	  widthE = track_box.size.width;
	  heightE = track_box.size.height;
	  mouseEvent = false;

	  //float h_val = 0, s_val = 0, v_val = 0;
	  segment->getParams (&minH,&maxH,0);
	  segment->getParams (&minS,&maxS,1);
	  segment->getParams (&minV,&maxV,2);

	  if ( (minH > 0) & (maxH > 0) )
 	    if ( (minS > 0) & (maxS > 0) )
 	      if ( (minV > 0) & (maxV > 0) )
		{
		  calibration tmp;
		  tmp.minH = minH;
		  tmp.maxH = maxH;
		  tmp.minS = minS;
		  tmp.maxS = maxS;
		  tmp.minV = minV;
		  tmp.maxV = maxV;
		  tmp.x = segPos.x;
		  tmp.y = segPos.y;
		  
		  list.push_back (tmp);
		} 
	}


      if (1)
	{
	  int x = element.position.x_pos;
	  int y = element.position.y_pos;

	  double minDistance = 10000000;
	  calibration tmp;

	  // Init
	  tmp.minH = 0;
	  tmp.maxH = 0;
	  tmp.minS = 0;
	  tmp.maxS = 0;
	  tmp.minV = 0;
	  tmp.maxV = 0;
	  
	  // Counting closest neighbours
	  int counter = 0;

	  for (vector<calibration>::iterator it = list.begin(); it!=list.end(); it++)
	    {
	      double distance = sqrt(pow(it->x - x,2) + pow(it->y - y,2));

	      if (distance < minDistance)
		{
		  minDistance = distance;
		  
		  minH = it->minH;
		  maxH = it->maxH;
		  minS = it->minS;
		  maxS = it->maxS; 
		  minV = it->minV;
		  maxV = it->maxV;
		}

	      int maxTMP = max (widthE, heightE);

	      if (distance < maxTMP)
		{
		  tmp.minH += it->minH;
		  tmp.maxH += it->maxH;
		  tmp.minS += it->minS;
		  tmp.maxS += it->maxS;
		  tmp.minV += it->minV;
		  tmp.maxV += it->maxV;
		  
		  counter ++;
		}	
	    }

	  if (counter > 1)
	    {
	      minH = tmp.minH/counter;
	      maxH = tmp.maxH/counter;
	      minS = tmp.minS/counter;
	      maxS = tmp.maxS/counter; 
	      minV = tmp.minV/counter;
	      maxV = tmp.maxV/counter;
	    }
            
	}

      cvCvtColor(src, hsv, CV_BGR2HSV);
      cvSplit( hsv, hue, sat, val, 0 );

      segment->doubleTh (hue,trackedH,minH,maxH);
      segment->doubleTh (sat,trackedS,minS,maxS);
      segment->doubleTh (val,trackedV,minV,maxV);

      //int smooth = (int)((49/2*(THRESHOLD))/255)*2+1;

      cvSmooth(trackedH,trackedH,CV_BLUR,smooth*2+1,0);
      cvSmooth(trackedS,trackedS,CV_BLUR,smooth*2+1,0);
      cvSmooth(trackedV,trackedV,CV_BLUR,smooth*2+1,0);
      //cvSmooth(movement,movement,CV_BLUR,smooth,0);

      cvMul(trackedH,trackedS,tracked,1.0/255.0);   
      cvMul(tracked,trackedV,tracked,1.0/(double)(THRESHOLD+1.0));

      // Weigh with motion!!
      //cvAddWeighted(movement, 10/255.0,tracked,1,1, tracked);

      //cvSmooth(tracked,tracked,CV_MEDIAN,5,0);
       
      int var = (10*(THRESHOLD+1))/255;

      cvDilate(tracked,tracked,NULL,var);
      cvErode(tracked,tracked,NULL,var);

      cvThreshold( tracked, tracked, THRESHOLD+1, 255, CV_THRESH_BINARY );
   

      // Locate item
      double points = element.find(tracked);
      int x = src->width/2, y = src->height/2;
      int orientation = 0;
      int size = 0;
      int inclination = -100;

      if (points > 100)
	{
	  int max_val;

	  element.getPosition (&x,&y);
	  track_box = element.getBox ();

	  cvGetMinMaxHistValue( H, 0, 0, 0, &max_val );

	  plot_box (track_box,src,max_val*180/hdims);
	  orientation = track_box.angle*100/90;

	  // Update means!!
	  widthE = 0.99*widthE+0.01*track_box.size.width;
	  heightE = 0.99*heightE+0.01*track_box.size.height;

	  // Compute deviations from means
	  size = -(100-track_box.size.width*50/widthE -track_box.size.height*50/heightE);
	  inclination = -100+ (-100*track_box.size.width/track_box.size.height+100*widthE/heightE)*4;

	  // Crop values!!
	  inclination = MAX (inclination,-100);
	  inclination = MIN (inclination,100);
	  size = MAX (size,-100);
	  size = MIN (size,100);
	  
	  // Autoupdate params??
	 //  if (points <200)
	  // 	    on_mouse( CV_EVENT_LBUTTONDOWN, x, y, NULL, NULL);

	}
      
      cvAdd(src,src,src,tracked);

      // Draw bars
      plot_text (src,"R - to reset",10,30);
      plot_bar (src,0,(char*)"O",orientation);
      plot_bar (src,2,(char*)"Z",size);
      plot_bar (src,4,(char*)"X",(x-src->width/2)*100/(src->width/2));
      plot_bar (src,3,(char*)"Y",-(y-src->height/2)*100/(src->height/2));
      plot_bar (src,1,(char*)"I",inclination);

      if (!cvGetWindowHandle("EasyTrack input"))
	{
	  break;
	  //cvShowImage( "EasyTrack input", src );

	  //cvCreateTrackbar("Sensitivity","EasyTrack input",&THRESHOLD,254,NULL);

	  //Dock window to tray!!!
	  //system ("xwininfo -name \"EasyTrack input\"|egrep \"Window id\"| awk '{print \"kdocker -w \" $4 \" -i icon.png &\"}'|sh ");

	}
      else
	{
	  cvShowImage( "EasyTrack input", src );
	  //cvShowImage( "Segmentation", dst );
	}

      js0->sendAll(orientation,inclination,(x-src->width/2)*100/(src->width/2),
		   -(y-src->height/2)*100/(src->height/2),size);

      char key = (char) cvWaitKey(10);
      if( key == 27 || key == 'q' || key == 'Q' ) // 'ESC'
	break;

      if ( key=='r' || key =='R')
	{
	  // Reset lists and params
	  while (!list.empty())
	    {
	      list.pop_back();
	    }
	      
	  minH = 0;
	  maxH = 0;
	  minS = 0;
	  maxS = 0;
	  minV = 0;
	  maxV = 0;
	}

      //      cvWriteFrame( pBGAvi, src );
    }

  // Release
  cvReleaseCapture( &OpenCV_CAM );
  segment->release();

  delete (js0);  
}

